﻿#include "stdafx.h"
#include "RobotClient.h"
#include "Helper.h"


void sendCmd(std::string cmd, char *respMsg)
{
	WORD wVersionRequested;
	WSADATA wsaData;//WSAata用来存储系统传回的关于WinSocket的资料。
	int err;

	wVersionRequested = MAKEWORD(1, 1);

	err = WSAStartup(wVersionRequested, &wsaData);
	if (err != 0) {
		return;
	}

	if (LOBYTE(wsaData.wVersion) != 1 || HIBYTE(wsaData.wVersion) != 1)
	{
		WSACleanup();
		return;
	}

	//初始化连接与端口号
	SOCKADDR_IN addrSrv;
	addrSrv.sin_addr.S_un.S_addr = inet_addr("127.0.0.1");//本机地址，服务器在本机开启
	addrSrv.sin_family = AF_INET;
	addrSrv.sin_port = htons(6600);// 设置端口号

	std::cout<<"Connecting to Remote..."<<std::endl;
	SOCKET sockClient = socket(AF_INET, SOCK_STREAM, 0);// AF_INET ..tcp连接
	connect(sockClient, (SOCKADDR*)&addrSrv, sizeof(SOCKADDR));//连接服务器
	std::cout<<"Connected!"<<std::endl;

	// 接收连接信息
	char recvBuf[256];
	while(recv(sockClient, recvBuf, 256, 0) == -1);
	std::cout<<recvBuf<<std::endl;

	// 发送指令
	send(sockClient, cmd.c_str(), cmd.size() + 1, 0);

	// 接收返回信息
	recv(sockClient, respMsg, 256, 0);
	std::cout<<respMsg<<std::endl;

	closesocket(sockClient);
	WSACleanup();
}

std::string ControlHand(HAND_STATUS status)    //控制手
{
    // CMD1 - Control Robot Hand
    std::string sCmd = "CMD1";

    switch (status) {
    case HAND_OPENED:
        sCmd += "1EOF";
        break;
    case HAND_CLOSED:
        sCmd += "0EOF";
        break;
    default:
        sCmd = "Hand Status Error!";
        break;
    }

    return sCmd;
}

std::string ControlGripper(GRIPPER_STATUS status)//控制手爪
{
    // CMD2 - Control Gripper
    std::string sCmd = "CMD2";

   /* switch (status) {
    case GRIPPER_OPENED:
        sCmd += "1EOF";
        break;
    case GRIPPER_CLOSED:
        sCmd += "0EOF";
        break;
    default:
        sCmd = "Gripper Status Error!";
        break;
    }*/

    return sCmd;
}
std::string ControlVavle(VAVLE_STATUS status)  //控制电磁阀
{
	// CMD6 - Control vavle
	std::string sCmd = "CMD3";

	//switch (status) {
	//case VAVLE_OPENED:
	//	sCmd += "1EOF";
	//	break;
	//case VAVLE_CLOSED:
	//	sCmd += "0EOF";
	//	break;
	//default:
	//	sCmd = "Vavle Status Error!";
	//	break;
	//}

	return sCmd;
}
std::string ControlPLC(PLC_STATUS status)
{
	// CMD30 - Control PLC
	std::string sCmd = "CMD30";

	switch (status) {
	case PLC_OPENED:
		sCmd += "1EOF";
		break;
	case PLC_CLOSED:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "PLC Status Error!";
		break;
	}

	return sCmd;
}

std::string ControlStepMotor(MOTOR_DIR dir)
{
    // CMD33 - Control Step Motor
    std::string sCmd = "CMD33"; // Control Step Motor - Run

    switch (dir) {
    case MOTOR_FORWARD:
        sCmd += "0EOF";
        break;
    case MOTOR_BACKWARD:
        sCmd += "1EOF";
        break;
    default:
        sCmd = "Step Motor Instruction Error!";
        break;
    }

    return sCmd;
}

//std::string ControlStepMotor_Stop(MOTOR_STATUS status)
//{
//    // CMD3 - Control Step Motor
//    std::string sCmd = "CMD30"; // Control Step Motor - Stop
//
//    switch (status) {
//    case MOTOR_ENALBED:
//        sCmd += "0EOF";
//        break;
//    case MOTOR_DISABLED:
//        sCmd += "1EOF";
//        break;
//    default:
//        sCmd = "Step Motor Stop Instruction Error!";
//        break;
//    }
//
//    return sCmd;
//}

std::string ControlStepMotor_Move(MOTOR_DIR dir, short cm, short mm)
{
    // CMD31 - Control Step Motor
    std::ostringstream ossCmd;

    ossCmd << "CMD31"; // Control Step Motor - Move

    switch (dir) {
    case MOTOR_FORWARD:
        ossCmd << "1";
        break;
    case MOTOR_BACKWARD:
        ossCmd << "0";
        break;
    default:
        return("Step Motor Stop Instruction Error!");
    }

    ossCmd << std::setfill('0');
    ossCmd << std::setw(2) << cm;
    ossCmd << std::setw(1) << mm;
    ossCmd << "EOF";

    return ossCmd.str();
}

std::string ControlStepMotor_Speed(short speed)
{
    // CMD3 - Control Step Motor
    std::ostringstream ossCmd;

    ossCmd << "CMD32"; // Control Step Motor - Speed

    ossCmd << std::setfill('0');
    ossCmd << std::setw(2) << speed;
    ossCmd << "EOF";

    return ossCmd.str();
}

std::string ControlRobot_SetSpeed(short speed)
{
    // CMD0 - Control Robot
    std::ostringstream ossCmd;

    ossCmd << "CMD012"; // Control Robot - Set - Speed

    ossCmd << std::setfill('0');
    ossCmd << std::setw(3) << speed;
    ossCmd << "EOF";

    return ossCmd.str();
}

std::string ControlRobot_SetJoint(float *joints, bool isSoftFloat, short sfIdx)
{
	// CMD0 - Control Robot
	std::ostringstream ossCmd;

	ossCmd << "CMD01"; // Control Robot - Set

	if(isSoftFloat)
	{
		ossCmd << (3 + sfIdx);// SOFTFLOAT[sfIdx]
	}
	else
	{
		ossCmd << "0";          // Current Position
	}

	ossCmd << "1"; // Joint

	ossCmd << "0"; //UF
	ossCmd << "8"; //UT

	ossCmd << "J";
	for(int i=0; i<9; i++)
	{
		if (i != 8) ossCmd << joints[i] <<",";
		else ossCmd << joints[i];
	}
	ossCmd << "J";

	ossCmd << "EOF";

	return ossCmd.str();
}

std::string ControlRobot_SetXyzwpr(float *xyzwpr)
{
	// CMD0 - Control Robot
	std::ostringstream ossCmd;

	ossCmd << "CMD0100"; // Control Robot - Set - Current Position - Xyzwpr

	ossCmd << "0"; //UF
	ossCmd << "0"; //UT

	ossCmd << "X";
	for(int i=0; i<9; i++)
	{
		if (i != 8) ossCmd << xyzwpr[i] <<",";
		else ossCmd << xyzwpr[i];
	}
	ossCmd << "X";

	short config[7] = {0, 0, 1, 1, 0, 0, 0};
	ossCmd << "G";
	for(int i=0; i<7; i++)
	{
		if (i != 6) ossCmd << config[i] <<",";
		else ossCmd << config[i];
	}
	ossCmd << "G";

	ossCmd << "EOF";

	return ossCmd.str();
}

std::string ControlRobot_GetCoord()
{
	// CMD0 - Control Robot
	std::ostringstream ossCmd;

	ossCmd << "CMD000"; // Control Robot - Get - Current Position
	ossCmd << "0"; //UF
	ossCmd << "8"; //UT
	ossCmd << "EOF";
	return ossCmd.str();
}

void Cmd2Coord(std::string cmd, float *pXyzwpr, short *pConfig, float *pJoints)
{
    // 接收指令头
    std::string sCmdHeader = cmd.substr(0,3);
    if (0 != sCmdHeader.compare("RTN"))  { return;	}

    // 检查控制对象
    //short sPosOrReg = cmd[3] - '0';

    // extract xyzwpr
    if(nullptr != pXyzwpr)
    {
        auto idx_x1 = cmd.find_first_of('X');
        auto idx_x2 = cmd.find_last_of('X');
        string2value(cmd.substr(idx_x1+1, idx_x2-idx_x1-1), pXyzwpr);
    }

    // extract config
    if(nullptr != pConfig)
    {
        auto idx_g1 = cmd.find_first_of('G');
        auto idx_g2 = cmd.find_last_of('G');
        string2value(cmd.substr(idx_g1+1, idx_g2-idx_g1-1), pConfig);
    }

    // extract joints
    if(nullptr != pJoints)
    {
        auto idx_j1 = cmd.find_first_of('J');
        auto idx_j2 = cmd.find_last_of('J');
        string2value(cmd.substr(idx_j1+1, idx_j2-idx_j1-1), pJoints);
    }
}
//
std::string Controlcylinder1(STATUS status)
{
	// CMDC1 - Control cylinder1
	std::string sCmd = "CMDC1";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder1 Status Error!";
		break;
	}

	return sCmd;
}
std::string Controlcylinder2(STATUS status)
{
	// CMDC2 - Control cylinder2
	std::string sCmd = "CMDC2";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder2 Status Error!";
		break;
	}

	return sCmd;
}
std::string Controlcylinder3(STATUS status)
{
	// CMDC3 - Control cylinder3
	std::string sCmd = "CMDC3";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder3 Status Error!";
		break;
	}

	return sCmd;
}
std::string Controlcylinder4(STATUS status)
{
	// CMDC4 - Control cylinder4
	std::string sCmd = "CMDC4";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder4 Status Error!";
		break;
	}
	return sCmd;
}
std::string Controlcylinder5(STATUS status)
{
	// CMDC5 - Control cylinder5
	std::string sCmd = "CMDC5";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder5 Status Error!";
		break;
	}
	return sCmd;
}
std::string Controlcylinder6(STATUS status)
{
	// CMDC6 - Control cylinder6
	std::string sCmd = "CMDC6";
	switch (status) {
	case ON:
		sCmd += "1EOF";
		break;
	case OFF:
		sCmd += "0EOF";
		break;
	default:
		sCmd = "cylinder6 Status Error!";
		break;
	}
	return sCmd;
}